#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     irsensor,       sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorLeft,     tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorArm,      tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorRight,    tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servoRight,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servoWristRight,      tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servoLeft,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servoWristLeft,       tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

//variables used for stall code
long timeStalling[3]; //amount of time the motors are stalling
int valueOfLastMove[3]; //the value of the encoders when the motor last moved
long timeStoppedMoving[3]; //the time the motors stopped

void InitializeStallCode()
{
	for(int i = 0; i <3; i++)
	{
		timeStalling[i] = 0;
		valueOfLastMove[i] = 0;
		timeStoppedMoving[i] = 0;
	}
}

int StallCode(int motorSentTo, int wantedPower)
{
	int motorIndex;//index value for the arrays
	switch(motorSentTo)//which motor power is being sent to
	{
		case motorLeft:
		motorIndex = 0;
		break;
		case motorRight:
		motorIndex = 1;
		break;
		case motorArm:
		default:
		motorIndex = 2;
		break;
	}
  int cur = nMotorEncoder[motorSentTo];//current encoder value of motor

if((wantedPower < -30 || wantedPower > 30)&&(cur == valueOfLastMove[motorIndex]))
{
	if(timeStalling[motorIndex] + 300 >= time1[T1])
	{
		if(timeStoppedMoving[motorIndex] == 0)
		{
			timeStoppedMoving[motorIndex] = time1[T1];
		}
		if(timeStoppedMoving[motorIndex] + 2500 >= time1[T1])
		{
			return 0;
		}
		else
		{
		    valueOfLastMove[motorIndex] = cur;
			timeStoppedMoving[motorIndex] = 0;
			timeStalling[motorIndex] = time1[T1];
		}
	}
	return wantedPower;
}
timeStalling[motorIndex] = time1[T1];
return wantedPower;
}

task main ()
{
    waitForStart(); //wait until FCS tells us to go
    InitializeStallCode();

    nMotorEncoder[motorLeft] = 0;

    while(nMotorEncoder[motorLeft] < (1440*5))
    {
    motor[motorLeft] = StallCode(motorLeft, 100);
    motor[motorRight] = StallCode(motorRight, 100);
    }
    motor[motorLeft] = StallCode(motorLeft, 0);
    motor[motorRight] = StallCode(motorRight,0);

    nMotorEncoder[motorLeft] = 0;

    motor[motorLeft] = StallCode(motorLeft, 100);
    motor[motorRight] = StallCode(motorRight, -100);
    wait1Msec(1200);
    motor[motorLeft] = StallCode(motorLeft, 0);
    motor[motorRight] = StallCode(motorRight,0);

    nMotorEncoder[motorLeft] = 0;

    while(nMotorEncoder[motorLeft] < (1440*2.5*1.05*1.15))
    {
    motor[motorLeft] = StallCode(motorLeft, 100);
    motor[motorRight] = StallCode(motorRight, 100);
    }
    motor[motorLeft] = StallCode(motorLeft, 0);
    motor[motorRight] = StallCode(motorRight,0);
    return;
    nMotorEncoder[motorLeft] = 0;

    while(nMotorEncoder[motorLeft] < (1440*0.5))
    {
    motor[motorLeft] = StallCode(motorLeft, 100);
    motor[motorRight] = StallCode(motorRight, 100);
    }
    motor[motorLeft] = StallCode(motorLeft, 0);
    motor[motorRight] = StallCode(motorRight,0);

    nMotorEncoder[motorLeft] = 0;

    while(nMotorEncoder[motorLeft] < (1440*0.5))
    {
    motor[motorLeft] = StallCode(motorLeft, 100);
    motor[motorRight] = StallCode(motorRight, 100);
    }
    motor[motorLeft] = StallCode(motorLeft, 0);
    motor[motorRight] = StallCode(motorRight,0);
}
